﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using RayDen.Library.Core.Primitives;

namespace RayDen.Library.Core
{
    using uint64 = UInt64;

    public static class Bits
    {
        const uint64 m1 = 0x5555555555555555; //binary: 0101...
        const uint64 m2 = 0x3333333333333333; //binary: 00110011..
        const uint64 m4 = 0x0f0f0f0f0f0f0f0f; //binary:  4 zeros,  4 ones ...
        const uint64 m8 = 0x00ff00ff00ff00ff; //binary:  8 zeros,  8 ones ...
        const uint64 m16 = 0x0000ffff0000ffff; //binary: 16 zeros, 16 ones ...
        const uint64 m32 = 0x00000000ffffffff; //binary: 32 zeros, 32 ones
        const uint64 hff = 0xffffffffffffffff; //binary: all ones
        const uint64 h01 = 0x0101010101010101; //the sum of 256 to the power of 0,1,2,3...


        public static int ones(uint64 x)
        {
            x = (x & m1) + ((x >> 1) & m1); //put count of each  2 bits into those  2 bits 
            x = (x & m2) + ((x >> 2) & m2); //put count of each  4 bits into those  4 bits 
            x = (x & m4) + ((x >> 4) & m4); //put count of each  8 bits into those  8 bits 
            x = (x & m8) + ((x >> 8) & m8); //put count of each 16 bits into those 16 bits 
            x = (x & m16) + ((x >> 16) & m16); //put count of each 32 bits into those 32 bits 
            x = (x & m32) + ((x >> 32) & m32); //put count of each 64 bits into those 64 bits 
            return (int)x;
        }

        public static int lzc(uint x)
        {
            x |= (x >> 1);
            x |= (x >> 2);
            x |= (x >> 4);
            x |= (x >> 8);
            x |= (x >> 16);
            return (int)(32u - ones(x));
        }


        //This uses fewer arithmetic operations than any other known  
        //implementation on machines with slow multiplication.
        //It uses 17 arithmetic operations.
        static int popcount_2(uint64 x)
        {
            x -= (x >> 1) & m1;             //put count of each 2 bits into those 2 bits
            x = (x & m2) + ((x >> 2) & m2); //put count of each 4 bits into those 4 bits 
            x = (x + (x >> 4)) & m4;        //put count of each 8 bits into those 8 bits 
            x += x >> 8;  //put count of each 16 bits into their lowest 8 bits
            x += x >> 16;  //put count of each 32 bits into their lowest 8 bits
            x += x >> 32;  //put count of each 64 bits into their lowest 8 bits
            return (int) (x & 0x7f);
        }

        //This uses fewer arithmetic operations than any other known  
        //implementation on machines with fast multiplication.
        //It uses 12 arithmetic operations, one of which is a multiply.
        static int popcount_3(uint64 x)
        {
            x -= (x >> 1) & m1;             //put count of each 2 bits into those 2 bits
            x = (x & m2) + ((x >> 2) & m2); //put count of each 4 bits into those 4 bits 
            x = (x + (x >> 4)) & m4;        //put count of each 8 bits into those 8 bits 
            return (int) ((x * h01) >> 56);  //returns left 8 bits of x + (x<<8) + (x<<16) + (x<<24) + ... 
        }
        public static uint ExpandBits(uint v)
        {
            v = (v * 0x00010001u) & 0xFF0000FFu;
            v = (v * 0x00000101u) & 0x0F00F00Fu;
            v = (v * 0x00000011u) & 0xC30C30C3u;
            v = (v * 0x00000005u) & 0x49249249u;
            return v;
        }

        /// <summary>
        /// Calculates 10-bit Morton Code for normalized 3D point (unit cube)
        /// </summary>
        /// <param name="x"> x-coordinate (0..1) </param>
        /// <param name="y"> y-coordinate (0..1)</param>
        /// <param name="z"> z-coordinate (0..1)</param>
        /// <returns> returns 10 bit 3D Morton code</returns>
        public static uint Morton3D(float x, float y, float z)
        {
            x = Math.Min(Math.Max(x * 1024.0f, 0.0f), 1023.0f);
            y = Math.Min(Math.Max(y * 1024.0f, 0.0f), 1023.0f);
            z = Math.Min(Math.Max(z * 1024.0f, 0.0f), 1023.0f);
            uint xx = ExpandBits((uint)x);
            uint yy = ExpandBits((uint)y);
            uint zz = ExpandBits((uint)z);
            //return Morton_3D_Encode_10bit(xx, yy, zz);

            return (xx << 2) | (yy <<1) | zz;
        }


        public static uint Morton3D(Vector v)
        {
            
            var lx = Int32Single.Create(v.x);
            var ly = Int32Single.Create(v.y);
            var lz = Int32Single.Create(v.z);


            
            var x =  (lx.u << 0);
            var y =  (ly.u << 0);
            var z =  (lz.u << 0);

            //return Morton_3D_Encode_10bit(x, y, z);

            var xx = ExpandBits((uint)x);
            var yy = ExpandBits((uint)y);
            var zz = ExpandBits((uint)z);
            return xx * 4 + yy * 2 + zz;
        }

        static uint Morton_3D_Encode_10bit(uint index1, uint index2, uint index3)
        { // pack 3 10-bit indices into a 30-bit Morton code
            index1 &= 0x000003ff;
            index2 &= 0x000003ff;
            index3 &= 0x000003ff;
            index1 |= (index1 << 16);
            index2 |= (index2 << 16);
            index3 |= (index3 << 16);
            index1 &= 0x030000ff;
            index2 &= 0x030000ff;
            index3 &= 0x030000ff;
            index1 |= (index1 << 8);
            index2 |= (index2 << 8);
            index3 |= (index3 << 8);
            index1 &= 0x0300f00f;
            index2 &= 0x0300f00f;
            index3 &= 0x0300f00f;
            index1 |= (index1 << 4);
            index2 |= (index2 << 4);
            index3 |= (index3 << 4);
            index1 &= 0x030c30c3;
            index2 &= 0x030c30c3;
            index3 &= 0x030c30c3;
            index1 |= (index1 << 2);
            index2 |= (index2 << 2);
            index3 |= (index3 << 2);
            index1 &= 0x09249249;
            index2 &= 0x09249249;
            index3 &= 0x09249249;
            return (uint)(index1 | (index2 << 1) | (index3 << 2));
        }

        static uint Morton_3D_Encode_10bit(int index1, int index2, int index3)
        { // pack 3 10-bit indices into a 30-bit Morton code
            index1 &= 0x000003ff;
            index2 &= 0x000003ff;
            index3 &= 0x000003ff;
            index1 |= (index1 << 16);
            index2 |= (index2 << 16);
            index3 |= (index3 << 16);
            index1 &= 0x030000ff;
            index2 &= 0x030000ff;
            index3 &= 0x030000ff;
            index1 |= (index1 << 8);
            index2 |= (index2 << 8);
            index3 |= (index3 << 8);
            index1 &= 0x0300f00f;
            index2 &= 0x0300f00f;
            index3 &= 0x0300f00f;
            index1 |= (index1 << 4);
            index2 |= (index2 << 4);
            index3 |= (index3 << 4);
            index1 &= 0x030c30c3;
            index2 &= 0x030c30c3;
            index3 &= 0x030c30c3;
            index1 |= (index1 << 2);
            index2 |= (index2 << 2);
            index3 |= (index3 << 2);
            index1 &= 0x09249249;
            index2 &= 0x09249249;
            index3 &= 0x09249249;
            return (uint)(index1 | (index2 << 1) | (index3 << 2));
        }
    }
}
